﻿using System;
using System.Collections;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.IO;
using MotionPlanning.MotionPlanningComponents;
using MotionPlanning.MotionPlanningComponents.Visualizations;
using MotionPlanning.MotionPlanningComponents.MotionPlanningSystems;
using MotionPlanning.MotionPlanningPrimitives;
using MotionPlanning.MotionPlanningAlgorithms;

namespace MotionPlanning.MotionPlanningAlgorithms
{
    abstract public class Planner_RRTBased
    {
        protected Trajectory solution;
        protected List<object> incrementalNodeArray;
        protected int iterations_total;
        protected Node x_root;
        protected double[] q_goal;
        protected double r_goal;
        protected RoboticSystemComponent System;
        protected PointSetComponent Vertices;
        protected SamplingComponent Sampler;
        protected VisualizationComponent Visualizer;
        protected bool doGoalCheck;
        protected string motionPlanningConfigFile;

        public ArrayList energyCurve;

        public Planner_RRTBased(RoboticSystemComponent system, VisualizationComponent visualizer, string motionPlanningConfigFile)
        {
            this.motionPlanningConfigFile = motionPlanningConfigFile;
            this.x_root = null;
            this.solution = null;
            this.incrementalNodeArray = new List<object>();
            this.System = system;
            this.System.LoadingSystemParameters(motionPlanningConfigFile);
            this.Sampler = new SamplingComponent();//(1000, 2000, 3000, 4000);
            this.Visualizer = visualizer;
            if (this.Visualizer != null)
            {
                this.Visualizer.SetHostPlanner(this);
            }
            this.Vertices = new PointSetComponent(
                this.System.Get_DOF_state(),
                this.System.Get_q_max(),
                this.System.Get_q_min(),
                this.System.Get_q_scale(),
                this.System.Get_q_topology(),
                13,
                false);
            this.LoadingAlgorithmSettings(this.motionPlanningConfigFile);
            iterations_total = 0;
            doGoalCheck = false;
            energyCurve = new ArrayList();
        }

        public void ResetPlanner()
        {
            this.Sampler = new SamplingComponent();
            this.Vertices = new PointSetComponent(
                this.System.Get_DOF_state(),
                this.System.Get_q_max(),
                this.System.Get_q_min(),
                this.System.Get_q_scale(),
                this.System.Get_q_topology(),
                13,
                true);
            this.LoadingAlgorithmSettings(this.motionPlanningConfigFile);
            iterations_total = 0;
            doGoalCheck = false;
            this.solution = null;
        }

        public bool LoadingAlgorithmSettings(string motionPlanningConfigFile)
        {
            TextReader tr = new StreamReader(motionPlanningConfigFile);
            if (tr == null)
                throw new Exception("input file cannot be opened.");
            string rawFile = tr.ReadToEnd();

            string[] separators = new string[] { "<PlanningAlgorithm>", "</PlanningAlgorithm>" };
            string[] entries = rawFile.Split(separators, StringSplitOptions.None);
            if (entries.Length != 3) return false;
            string algorithmSettingsText = entries[1];

            separators = new string[] { "<Algorithm_Type>", "</Algorithm_Type>" };
            entries = algorithmSettingsText.Split(separators, StringSplitOptions.None);
            if (entries.Length != 3) return false;
            //this.Algorithm_Type = entries[1];

            separators = new string[] { "<x_start>", "</x_start>" };
            entries = algorithmSettingsText.Split(separators, StringSplitOptions.None);
            if (entries.Length != 3) return false;
            double[] x_start = MotionPlanningUtils.StringToDoubleArray(entries[1]);
            this. SetRoot(x_start);

            separators = new string[] { "<q_goal>", "</q_goal>" };
            entries = algorithmSettingsText.Split(separators, StringSplitOptions.None);
            if (entries.Length != 3) return false;
            this.q_goal = MotionPlanningUtils.StringToDoubleArray(entries[1]);

            separators = new string[] { "<r_goal>", "</r_goal>" };
            entries = algorithmSettingsText.Split(separators, StringSplitOptions.None);
            if (entries.Length != 3) return false;
            this.r_goal = MotionPlanningUtils.StringToDoubleArray(entries[1])[0];

            tr.Close();
            return true;
        }

        public SamplingComponent GetSampler()
        {
            return Sampler;
        }
        public Trajectory GetSolution()
        {
            return this.solution;
        }
        public PointSetComponent GetVertices()
        {
            return Vertices;
        }
        public RoboticSystemComponent GetSystem()
        {
            return System;
        }
        public VisualizationComponent GetVisualizer()
        {
            return Visualizer;
        }
        public virtual void Plot()
        {
            List<object> graph = Vertices.GetAllDataObjects();
            Visualizer.PlotVerticesWithCost(graph);
            //Visualizer.PlotVertices(graph);
        }
        public virtual bool SaveMemoryToDisk(string filename)
        {
            TextWriter tw = new StreamWriter(filename, false);
            string fileContent = "";
            fileContent += "<MotionPlanning>\n";
            fileContent += "<SamplingComponent>" + Sampler.ToString() + "</SamplingComponent>\n";
            fileContent += "<RoboticSystem>" + System.ToString() + "</RoboticSystem>\n";
            fileContent += "<PlanningAlgorithm>" + this.ToString() + "</PlanningAlgorithm>\n";
            tw.Write(fileContent);
            List<object> train = Vertices.GetAllDataObjects();
            if (train.Count != 0)
            {
                fileContent = "<GraphComponent>\n";
                tw.Write(fileContent);
                fileContent = "\t<PointSetComponent.Depth>" + Vertices.GetDepth()+"</PointSetComponent.Depth>\n";
                tw.Write(fileContent);
                //save vertices
                fileContent = "<Vertices>\n";
                fileContent += "\t<NumberVertices>" + train.Count + "</NumberVertices>\n";
                tw.Write(fileContent);
                int nEdges = 0;
                for (int i = 0; i < train.Count; ++i)
                {
                    fileContent = "";
                    Node node = (Node)train[i];
                    string data = node.get_Value().ToString();
                    fileContent += "<v>";
                    fileContent += data;
                    fileContent += "</v>\n";
                    tw.Write(fileContent);
                    List<Edge> children = node.get_Children();
                    if (children != null)
                        nEdges += node.get_Children().Count;
                }
                fileContent = "</Vertices>\n";
                tw.Write(fileContent);
                //save edges
                Hashtable nodeToIndexTable = PointSetComponent.NodeToIndexOnTrain(train);
                fileContent = "<Edges>\n";
                fileContent += "\t<NumberEdges>" + nEdges + "</NumberEdges>\n";
                tw.Write(fileContent);
                for (int i = 0; i < train.Count; ++i)
                {
                    List<Edge> children = ((Node)train[i]).get_Children();
                    if (children == null) continue;
                    for (int j = 0; j < children.Count; ++j)
                    {
                        fileContent = "";
                        fileContent += "\t<e>";
                        Edge edge = children[j];
                        Node entry = edge.get_entry();
                        Node exit = edge.get_exit();
                        int idx_entry = (int)nodeToIndexTable[entry];
                        int idx_exit = (int)nodeToIndexTable[exit];
                        fileContent += idx_entry + "," + idx_exit + ",";
                        fileContent += edge.get_Value().ToString();
                        fileContent += "</e>\n";
                        tw.Write(fileContent);
                    }
                }
                fileContent = "</Edges>\n";
                tw.Write(fileContent);
                fileContent = "</GraphComponent>\n";
                tw.Write(fileContent);
            }
            fileContent = "</MotionPlanning>";
            tw.Write(fileContent);
            tw.Close();
            return true;
        }
        public int Get_iterations_total()
        {
            return iterations_total;
        }

        public void SetRoot(double[] q_start)
        {
            x_root = SpawnNode_Like(q_start);
            this.Vertices.AddPoint(x_root.get_Value().get_q(), x_root);
            //reset samplers
            //reset vertices
        }
        public Node GetRoot()
        {
            return x_root;
        }
        public double[] Get_q_goal()
        {
            return this.q_goal;
        }
        public double Get_r_goal()
        {
            return this.r_goal;
        }
        public bool IsGoalCheckTrue()
        {
            return doGoalCheck;
        }
        abstract public Node SpawnNode_Like(double[] q);
        abstract public Node SpawnNode_As(double[] q);
        abstract public Edge SpawnEdge();
        abstract public Edge SpawnEdge(Node entry, Node exit);

        abstract public void Execute(int n_iterations);
        abstract public override string ToString();
        abstract public void Test();
        abstract public bool GoalReached(double[] q_test);

        virtual public void SetGoal(double[] q_goal)
        {
            throw new NotImplementedException();
        }

        public void get_statistics(ref int n_nodes, ref double energy_averaged, ref double coverage_measure)
        {
            List<object> train = this.Vertices.GetAllDataObjects();
            n_nodes = train.Count;


            energy_averaged = 0;
            for (int i = 0; i < n_nodes; ++i)
            {
                if (train[i] == null) continue;
                double cc = ((Node)train[i]).get_Value().get_utility_c();
                energy_averaged += cc / n_nodes;
            }

            int res_x=64;
            int res_y=64;
            int[,] cells = new int[res_x, res_y];
            for (int i = 0; i < n_nodes; ++i)
            {
                if (train[i] == null) continue;
                double[] q = ((Node)train[i]).get_Value().get_q();
                int idx_x = (int)Math.Floor((q[0] - System.Get_q_min()[0]) / (System.Get_q_max()[0] - System.Get_q_min()[0]) * res_x);
                int idx_y = (int)Math.Floor((q[1] - System.Get_q_min()[1]) / (System.Get_q_max()[1] - System.Get_q_min()[1]) * res_y);
                cells[idx_y, idx_y]++;
            }
            double E_X_ = (double)n_nodes / (res_x * res_y);
            double E_X2_ = 0;
            for(int i=0;i<res_x;++i)
                for (int j = 0; j < res_y; ++j)
                {
                    double v_ij = (double)cells[i, j] * cells[i, j] / (res_x * res_y);
                    E_X2_ += v_ij;
                }
            coverage_measure = E_X2_ - E_X_ * E_X_;
        }

        public void get_system_average_energy(ref int n_nodes, ref double energy_averaged)
        {
            List<object> train = this.Vertices.GetAllDataObjects();
            n_nodes = train.Count;
            energy_averaged = 0;
            for (int i = 0; i < n_nodes; ++i)
            {
                if (train[i] == null) continue;
                double cc = ((Node)train[i]).get_Value().get_utility_c();
                energy_averaged += cc / n_nodes;
            }
        }
        public void EnergyReporting(int everyNIterations)
        {
            if (iterations_total % everyNIterations == 0)
            {
                int n_nodes = 0;
                double energy_averaged = 0;
                get_system_average_energy(ref n_nodes, ref energy_averaged);
                double[] element = new double[3];
                element[0] = n_nodes;
                element[1] = energy_averaged;
                element[2] = iterations_total;
                this.energyCurve.Add(element);
            }
        }

        public bool Propagation(double[] q_start, double[] q_target, ref double[] u, ref double[] q_end, ref double step_cost)
        {
            throw new NotImplementedException();
        }

        virtual public string MonitorPlannerState()
        {
            throw new NotImplementedException("MonitorPlannerState not implemented.");
        }

        public double[] NodeSyncForAlias(Node x)
        {
            RoboticSystemPlanningNodeData node_value = x.get_Value();
            double[] q_alias = null;
            System.Calculate_q_alias(node_value.get_q(), ref q_alias);
            node_value.set_q_alias(q_alias);
            return q_alias;
        }
    }
}
